﻿/*
 * HCC Robotica, Workshop Robot programmeren met C# 
 * Oefening : wo_06	Robot, een rondje rijden 
 * Naam     : Hinnie
 * Versie   : september 2011
 */

using System;
using System.Threading;
using System.IO.Ports;
using System.Text;

using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;

using GHIElectronics.NETMF.FEZ;

namespace wo_06
{
    public class wo_06
    {
        public static void Main()
        {

            OutputPort LED = new OutputPort((Cpu.Pin)FEZ_Pin.Digital.LED, false);
            InputPort ButtonLDR = new InputPort((Cpu.Pin)FEZ_Pin.Digital.LDR, false, Port.ResistorMode.PullUp);
            InputPort ButtonDi2 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di2, false, Port.ResistorMode.Disabled);
            InputPort ButtonDi4 = new InputPort((Cpu.Pin)FEZ_Pin.Digital.Di4, false, Port.ResistorMode.Disabled);
            SerialPort ComPort = new SerialPort("COM2", 115200); // TODO: Hier je eigen waardes invullen.
            bool ledState = false;
            bool ledSwitch = false;
            const int loopTime = 5;
            const int flashTime = 200 / loopTime;
            const sbyte startSpeed = 75;

            bool butLDR = true;
            bool butDi2 = true;
            bool butDi4 = true;
            sbyte leftMotor = startSpeed;
            sbyte rightMotor = (sbyte)(startSpeed * 0.60);
            sbyte lastSpeed = 0;

            ComPort.Open();
            byte[] txBuf;
            string Msg = "";

            for (int i = 0; i < 2; i++)
            {
                FEZMini_Robot.Beep(8000, 50);
                Thread.Sleep(100);
            }
            while (ButtonLDR.Read() == butLDR)
            {
                ledSwitch = !ledSwitch;
                LED.Write(ledSwitch);                   // less intensity onboard LED
                Thread.Sleep(loopTime);
            }
            FEZMini_Robot.Beep(8000, 100);
            while (ButtonLDR.Read() != butLDR) { }

            int flashCount = 0;
            while (true)
            {
                if (ButtonDi2.Read() != butDi2)         // circle to less 
                {
                    butDi2 = ButtonDi2.Read();
                    if (!butDi2)
                    {
                        FEZMini_Robot.Beep(7200, 50);
                        rightMotor--;
                    }
                }
                else if (ButtonDi4.Read() != butDi4)    // circle to large
                {
                    butDi4 = ButtonDi4.Read();
                    if (!butDi4)
                    {
                        FEZMini_Robot.Beep(7600, 50);
                        rightMotor++;
                    }
                }
                if (rightMotor != lastSpeed)
                {
                    lastSpeed = rightMotor;
                    Msg = "Speed Left= " + leftMotor.ToString() + " Speed Right= " + rightMotor.ToString() + "\n\r";
                    txBuf = Encoding.UTF8.GetBytes(Msg);
                    ComPort.Write(txBuf, 0, txBuf.Length);
                }

                FEZMini_Robot.Move(leftMotor, rightMotor);
                if (flashCount++ >= flashTime)
                {
                    flashCount = 0;
                    ledState = !ledState;       // toggle LED state
                    LED.Write(ledState);        // blink onboard LED
                }
                if (ledState)
                {
                    ledSwitch = !ledSwitch;
                    LED.Write(ledSwitch);       // less intensity onboard LED
                }
                Thread.Sleep(loopTime);
            }
        }
    }
}
